#include "stdafx.h"
#include "SteeringBehaviors.hpp"
#include "Simulation.hpp"
#include "RandGenFast.hpp"
#include "PropertyManager.hpp"
#include "Common.hpp"

//////////////////////////////////////////////////////////////////////////

SteeringBehaviors::SteeringBehaviors()
	: _wanderTarget(vec3::ZERO), _wanderTargetProj(vec3::ZERO)
{

}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::separation(Agent* agent)
{
	vec3 force = vec3::ZERO;
	vec3 agentVec;
	const Simulation::AgentVecT& neighbors = Simulation::Get().getAgents();

	for (int i = 0; i < (int)neighbors.size(); ++i)
	{
		if (neighbors[i]->isTagged())
		{
			agentVec = agent->getPosition() - neighbors[i]->getPosition();
			force += agentVec.normalisedCopy() / agentVec.length();
		}
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::cohesion(Agent* agent, float maxVelocity)
{
	vec3 force = vec3::ZERO;
	vec3 geomCenter = vec3::ZERO;
	int neighborCount = 0;
	const Simulation::AgentVecT& neighbors = Simulation::Get().getAgents();

	for (int i = 0; i < (int)neighbors.size(); ++i)
	{
		if (neighbors[i]->isTagged())
		{
			geomCenter += neighbors[i]->getPosition();
			++neighborCount;
		}
	}

	if (neighborCount > 0)
	{
		geomCenter /= (float)neighborCount;

		vec3 geomCenterVec = geomCenter - agent->getPosition();
		vec3 desiredVel = geomCenterVec.normalisedCopy() * maxVelocity;
		force = desiredVel - agent->getVelocity();
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::alignment(Agent* agent)
{
	vec3 force = vec3::ZERO;
	vec3 avgHeading = vec3::ZERO;
	int neighborCount = 0;
	const Simulation::AgentVecT& neighbors = Simulation::Get().getAgents();

	for (int i = 0; i < (int)neighbors.size(); ++i)
	{
		if (neighbors[i]->isTagged())
		{
			avgHeading += neighbors[i]->getHeading();
			++neighborCount;
		}
	}

	if (neighborCount > 0)
	{
		avgHeading /= (float)neighborCount;
		force = avgHeading - agent->getHeading();
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::wander(Agent* agent, float dt, float jitter, float radius, float distance)
{
	vec3 force = vec3::ZERO;

	float jitterDt = dt * jitter;
	_wanderTarget += vec3(
		rnd.RandFloat(-jitterDt, jitterDt),
		rnd.RandFloat(-jitterDt, jitterDt)/2,
		rnd.RandFloat(-jitterDt, jitterDt) );
	_wanderTarget = _wanderTarget.normalisedCopy() * radius;

	_wanderTargetProj = agent->getPosition() + agent->getHeading() * distance + _wanderTarget;
	force = _wanderTargetProj - agent->getPosition();

	if (agent->getPosition().length() > (Globals::SCENE_SIZE_X/2 * 0.8f))
	{
		force = (force + -agent->getPosition().normalisedCopy() * force.length()) / 2;
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::evade(Agent* agent, float maxVelocity, float turnAround, float timeScale)
{
	vec3 force = vec3::ZERO;
	const Simulation::AgentVecT& neighbors = Simulation::Get().getAgents();
	
	if (!neighbors.empty())
	{
		vec3 evadeTarget = vec3::ZERO;
		int evadeCount = 0;

		for (int i = 0; i < (int)neighbors.size(); ++i)
		{
			if (neighbors[i]->isTagged())
			{
				vec3 agentVec = neighbors[i]->getPosition() - agent->getPosition();
				float time = agentVec.length() / (neighbors[i]->getVelocity().length() + maxVelocity);
				float turnAroundTime = (agent->getHeading().dotProduct(agentVec.normalisedCopy() * -1) - 1.0f)
					* -turnAround;
				time += turnAroundTime;
				time *= timeScale;

				evadeTarget += neighbors[i]->getPosition() + neighbors[i]->getVelocity() * time;
				++evadeCount;
			}
		}

		evadeTarget /= (float)evadeCount;

		vec3 desiredVel = (agent->getPosition() - evadeTarget).normalisedCopy() * maxVelocity;
		force = desiredVel - agent->getVelocity();
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::pursuit(Agent* agent, float maxVelocity, float timeScale)
{
	vec3 force = vec3::ZERO;
	const Simulation::AgentVecT& neighbors = Simulation::Get().getAgents();

	if (!neighbors.empty())
	{
		vec3 pursuitTarget = vec3::ZERO;
		int pursuitCount = 0;

		for (int i = 0; i < (int)neighbors.size(); ++i)
		{
			if (neighbors[i]->isTagged())
			{
				vec3 agentVec = neighbors[i]->getPosition() - agent->getPosition();
				float time = agentVec.length() / (neighbors[i]->getVelocity().length() + maxVelocity);
				time *= timeScale;

				pursuitTarget += neighbors[i]->getPosition() + neighbors[i]->getVelocity() * time;
				++pursuitCount;
			}
		}

		pursuitTarget /= (float)pursuitCount;

		vec3 desiredVel = (pursuitTarget - agent->getPosition()).normalisedCopy() * maxVelocity;
		force = desiredVel - agent->getVelocity();
	}

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::evadeAgent(Agent* agent, Agent* evadedAgent, float maxVelocity, float turnAround, float timeScale)
{
	vec3 force = vec3::ZERO;

	vec3 agentVec = evadedAgent->getPosition() - agent->getPosition();
	float time = agentVec.length() / (evadedAgent->getVelocity().length() + maxVelocity);
	float turnAroundTime = (agent->getHeading().dotProduct(agentVec.normalisedCopy() * -1) - 1.0f)
		* -turnAround;
	time += turnAroundTime;
	time *= timeScale;

	vec3 evadeTarget = evadedAgent->getPosition() + evadedAgent->getVelocity() * time;
	vec3 desiredVel = (agent->getPosition() - evadeTarget).normalisedCopy() * maxVelocity;
	force = desiredVel - agent->getVelocity();

	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

vec3 SteeringBehaviors::separateAgent(Agent* agent, Agent* sepAgent)
{
	vec3 force = agent->getPosition() - sepAgent->getPosition();
	return force.normalisedCopy();
}

//////////////////////////////////////////////////////////////////////////

void SteeringBehaviors::resetWanderTarget()
{
	_wanderTarget = vec3::ZERO;
	_wanderTargetProj = vec3::ZERO;
}

//////////////////////////////////////////////////////////////////////////